package org.peno.goud;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;

import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.TachoPilot;

/*
 * TODO: zie LightSensor.java
 * - de raw values liggen tussen 145 (donker) en 890 (zonlicht)
 * - om genormaliseerd te werken (dus waarden tussen 0 en 100):
 * 		- calibreer met calibrateLow en calibrateHigh OF setLow(int low) en setHigh (int high)
 * 			indien je zelf waarden wilt geven (bvb: opgeslagen calibratie-waarden)
 * 		- gebruik getLightValue() ( == readValue() , meer overhead) om dan de genormaliseerde waarde uit te lezen
 * 
 */
public class LightReader extends Thread {
	
	static LightSensor sensor = new LightSensor(SensorPort.S1);
	static final TachoPilot pilot = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C, true);
	// samplingPeriod in milliseconds
	static final int samplingPeriod = 20;
	// retentie periode in milliseconds
	static final int bufferRetentionPeriod = 2000;
	static final int transitionDetectionPeriod = 100;
	static final float RC = 2;
	
	int[] signalBuffer;
	int[] bars = new int[7];
	int barcodeCounter;
	float alpha;
	boolean readingStrip;
	public int BROWN = 508;
	public int WHITE = 459;
	public int BLACK = 684;
	private int state;
	int second = BROWN;
	static BTConnection connection;
	static DataInputStream dataIn;
	static DataOutputStream dataOut;
//	static DataOutputStream fileOut;
//	static FileOutputStream out;
//	static File data;

	public LightReader() {
		// roep constructor van superclass aan ( Thread() )
		super();
		signalBuffer = new int[bufferRetentionPeriod / samplingPeriod];
		alpha = 1.0f;
		readingStrip = false;
	}
	
	public static void main(String[] args)
	{	
		LightReader reader = new LightReader();
		LightReader.sensor.setFloodlight(true);	
		connect();
		
		System.out.println("Press ENTER to calibrate BLACK");
		Button.ENTER.waitForPressAndRelease();
		reader.BLACK = SensorPort.S1.readRawValue();
		System.out.println(reader.BLACK);
		
		System.out.println("Press ENTER to calibrate BROWN");
		Button.ENTER.waitForPressAndRelease();
		reader.BROWN = SensorPort.S1.readRawValue();
		System.out.println(reader.BROWN);
		
		System.out.println("Press ENTER to calibrate WHITE");
		Button.ENTER.waitForPressAndRelease();
		reader.WHITE = SensorPort.S1.readRawValue();
		System.out.println(reader.WHITE);
		
		System.out.println("Press ENTER to start driving");
		Button.ENTER.waitForPressAndRelease();
		LCD.clear();
		pilot.forward();
		reader.fillSignalBuffer(reader.BROWN);
		reader.start();
		
		Button.ENTER.waitForPressAndRelease();
		connection.close();
		System.out.println("file stream closed");
		pilot.stop();
		
//		Button.LEFT.addButtonListener(new ButtonListener() {
//			public void buttonPressed(Button b) {
//				
//			}
//
//			public void buttonReleased(Button b) {
//				pilot.stop();
//			}
//		});
//		Button.RIGHT.addButtonListener(new ButtonListener() {
//			public void buttonPressed(Button b) {
//				
//			}
//
//			public void buttonReleased(Button b) {
//				pilot.forward();
//			}
//		});
//		Button.ENTER.addButtonListener(new ButtonListener() {
//			public void buttonPressed(Button b) {
//				
//			}
//
//			public void buttonReleased(Button b) {
//				pilot.stop();
//			}
//		});

	}

	public void run(){
		try {
			
			for (;;) {
				Thread.sleep(samplingPeriod);
				int lightValue = SensorPort.S1.readRawValue();	
				if (detectTransition()) {
					readingStrip = !readingStrip;
					if (!readingStrip) {

						if (barcodeCounter < 100) {
							System.out.println("The counter is: " + barcodeCounter);
							interpretBarcode();
							System.out.println("The barcode is: ");
							System.out.println(bars[0] +"-"+ bars[1] +"-"+ bars[2] +"-"+ bars[3] +"-"+ bars[4] +"-"+ bars[5] +"-"+ bars[6]);
						}
						
						// normaal niet nodig, voor de zekerheid.
						barcodeCounter = 0;
						
					} else {
						barcodeCounter = 0;
					}
				}
				
				push(lightValue);
			}
			
		} catch (InterruptedException e) {
			LCD.clear();
			System.out.println("run() interrupted");
		}
	}
	
	private void interpretBarcode()
	{
		int barInterval = barcodeCounter / 7;
		
		int j = 0;
		int limit = signalBuffer.length - (transitionDetectionPeriod/samplingPeriod) - barcodeCounter + barInterval/2;
		for (int i = limit; i < limit + barcodeCounter && j < 7; i += barInterval) {
			bars[j] = (signalBuffer[i] < (WHITE + BLACK)/2) ? 1 : 0;
			j++;
		}
	}
	
	// als zwart == 1
	private int[] correctBarCode(int[] barCode)
	{
		int[][] correctBarCodes = {{1,1,1,1,1,1,1},{1,1,1,0,0,0,0},{1,1,0,1,0,0,1},{1,1,0,0,1,1,0},{1,0,1,1,0,1,0},{1,0,1,0,1,0,1},{1,0,0,1,1,0,0},{1,0,0,0,0,1,1},{0,1,1,1,1,0,0},{0,1,1,0,0,1,1},{0,1,0,1,0,1,0},{0,1,0,0,1,0,1},{0,0,1,1,0,0,1},{0,0,1,0,1,1,0},{0,0,0,1,1,1,1},{0,0,0,0,0,0,0}};
		int[] distance = new int[16];
		int smallestDistance=7;
		int smallestIndex=0;

		//bereken distance voor elke barcode
		for(int i = 0; i<16; i++){
			for(int k = 0; k<7; k++){
			distance[i] += Math.abs(correctBarCodes[i][k]-barCode[k]);
			}
		}
		
		//zoek index met kleinste distance
		for(int i = 0; i<16; i++){
			if(distance[i] == 0){
				//distance 0 -> kleinste distance
				smallestIndex = i;
				break;
			}
			if(distance[i]<smallestDistance){
				smallestDistance = distance[i];
				smallestIndex = i;
			}
		}

		return correctBarCodes[smallestIndex];
	}
	
	private boolean detectTransition()
	{
		int numDetectionSamples = transitionDetectionPeriod / samplingPeriod;
		int numPresamples = numDetectionSamples;
		//int mean = bufferMean(signalBuffer.length - numDetectionSamples - numPresamples, signalBuffer.length - 1 - numPresamples);
		int mean = signalBuffer[signalBuffer.length - 1 - numPresamples];
		//int preMean = bufferMean(signalBuffer.length - numDetectionSamples - numPresamples, signalBuffer.length - numDetectionSamples);
		int preMean = bufferMean(signalBuffer.length  - 2*numPresamples, signalBuffer.length - 2 - numPresamples);
		//int postMean = bufferMean(signalBuffer.length - numPresamples, signalBuffer.length - 1);
		int postMean = bufferMean(signalBuffer.length - numPresamples, signalBuffer.length - 1);
		
		boolean meanBrown = (mean < BROWN * 1.05f) && (mean > BROWN *0.95f);
		boolean meanBlack = (mean < BLACK * 1.05f) && (mean > BLACK *0.95f);
		boolean meanWhite = (mean < WHITE * 1.05f) && (mean > WHITE *0.95f);
		
		boolean premeanBrown = (preMean < BROWN * 1.05f) && (preMean > BROWN *0.95f);
		
		// eventueel een kortere periode voor pre- en postMean Black & White; het moet binnen 1 bar-streepje passen
		// TODO: check hoeveel samples hij op hellingen heeft en hoe veel tijd er verloopt in 1 barcode-streepje (# samples * sampleperiod)
		// TODO: overloop de samples met een timeframe voor mean en kijk wat er gebeurt (of het voldoet aan ons algoritme)
		boolean postmeanBrown = (postMean < BROWN * 1.05f) && (postMean > BROWN *0.95f);
		boolean postmeanBlack = (postMean < BLACK * 1.05f) && (postMean > BLACK *0.95f);
		boolean postmeanWhite = (postMean < WHITE * 1.05f) && (postMean > WHITE *0.95f);
		
		boolean transition = false;
		
		switch (state) {
		case 0: // 0 = bruin
			if ((meanBlack || meanWhite) && premeanBrown && (postmeanBlack || postmeanWhite)) {
				transition = true;
			}
			break;
		case 1: // 1 = zwart
			if (preMeanBlack && meanBrown && postmeanBrown) {
				transition = true;
			}
			break;
			
		case 2: // 2 = wit
			if (meanBrown && postmeanBrown) {
				transition = true;
			}
			break;
	
		default:
			break;
		}
		
		if (meanBrown) {
			state = 0;
		} else if(meanBlack) {
			state = 1;
		} else if(meanWhite) {
			state = 2;
		}
		
		try {
			dataOut.writeInt(state);
			dataOut.writeInt(transition? 1 : 0);
			dataOut.flush();
		} catch (IOException e) {
	
		}
		
		return transition;
		
	}

	private void push(int lightValue)
    {
		// filter: zie http://en.wikipedia.org/wiki/Low-pass_filter#Discrete-time_realization
		for (int i = 0; i < signalBuffer.length-1; i++) {
			signalBuffer[i] = signalBuffer[i + 1];
		}
//		signalBuffer[signalBuffer.length-1] = (int) ((alpha * lightValue) + (1 - alpha) * signalBuffer[signalBuffer.length-1]);
		signalBuffer[signalBuffer.length-1] = lightValue;
		
		if (readingStrip) {
			if (barcodeCounter > 100) {
				barcodeCounter = 0;
				readingStrip = false;
			} else {
				barcodeCounter++;
			}
		}
		
		try {
			dataOut.writeInt(signalBuffer[signalBuffer.length-1]);
			dataOut.flush();
		} catch (IOException e) {

		}
    }
	
	private int bufferMean(int beginIndex, int endIndex)
    {
    	int sum = 0;  // sum of all the elements
    	for (int i = beginIndex; i <= endIndex; i++) {
    		sum += signalBuffer[i];
		}
        return sum / (endIndex - beginIndex + 1);

    }
	
	public void fillSignalBuffer(int value)
	{
		for (int i = 0; i < signalBuffer.length; i++) {
			signalBuffer[i] = value;
		}
	}
	
	/**
	 * Estabish bluetooth connection to mission control
	 */
	public static void connect()
	{
		LCD.clear();
		LCD.drawString("Waiting", 0, 0);
		connection = Bluetooth.waitForConnection(); // this method is very patient. 
		LCD.clear();
		LCD.drawString("Connected", 0, 0);
		dataIn = connection.openDataInputStream();
		dataOut = connection.openDataOutputStream();
		
//		data = new File("lightLog.dat");
//		try {
//			data.createNewFile();
//			out = new FileOutputStream(data);
//		} catch(IOException e) {}
//		fileOut = new DataOutputStream(out);


	}
}